package simulator.meta.agent;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simbad.sim.Agent;
import util.essential.SourceGetter;

public abstract class CopyOfControlledAgent extends Agent{
	//Constants:
	protected final int STEP_INCREASE = 1;
	protected final int TURN_INCREASE = 50;
	protected final boolean GOING_FORTH = true;
	protected final boolean TURNNING_RIGHT = true;
	
	//Attributes:
	protected int step;
	protected int turn;
	protected boolean stepStatus;
	protected boolean turnningStatus;
//	private double lastX,lastZ;
	private int beginingValue,endingValue;
	private int stepForth;
	private double azimuth;
	
	//Constructor:
	public CopyOfControlledAgent(Vector3d arg0, String arg1) {
		super(arg0, arg1);
		//Setting current position:
//		this.lastX = arg0.getX()-10;
//		this.lastZ = arg0.getZ();
		this.stepStatus = this.GOING_FORTH;
		this.azimuth = 0;
	}
	
	//Methods:
	public void setAwarenessAboutDisplacementWithinTheWorld(float worldsize,int numberOfTiles){
		SourceGetter.out.debug("worldsize = "+worldsize+", numberOfTiles = "+numberOfTiles);
		/*this.beginingValue = (int) (((worldsize*numberOfTiles)-(2*worldsize))/(2*numberOfTiles));
		this.endingValue = -1*this.beginingValue;
		*/
		//this.beginingValue = (int) ((worldsize/2)-((worldsize/numberOfTiles)/2));
		this.beginingValue = (int) (worldsize/2);
		//this.beginingValue = (int) ((Math.pow(worldsize, 2)-(2*worldsize))/(2*numberOfTiles));
		this.endingValue = -1*this.beginingValue;
		this.stepForth = (int)(worldsize/numberOfTiles);
	}
	private boolean areIntoRange(double x,double z){
		SourceGetter.out.debug(	"("+x+">="+this.beginingValue+"&&"+x+"<="+this.endingValue+"&&"+
								z+">="+this.beginingValue+"&&"+z+"<="+this.endingValue+")");
		return (x<=this.beginingValue && x>=this.endingValue&&
				z<=this.beginingValue && z>=this.endingValue);
	}
	public void stepUp(){
//		if(this.stepStatus!=this.GOING_FORTH&&this.step==(-1*this.STEP_INCREASE)){
//			this.step=0;
//			this.stepStatus = this.GOING_FORTH;
//		}else{
//			this.step+=(this.step>3)?0:this.STEP_INCREASE;
//		}
		Point3d p = new Point3d();
		this.getCoords(p);
		double 	x = p.getX(),
				z = p.getZ(),
				azimut = this.getAzimuth(),
				extra = 0.0d;
		
		if(azimut == 360 || azimut == 0){
			x+=(this.stepForth+extra);
		}else if(azimut == 180){
			x-=(this.stepForth+extra);
		}else if(azimut == 90){
			z-=(this.stepForth+extra);
		}else if(azimut == 270){
			z+=(this.stepForth+extra);
		}
		SourceGetter.out.debug("ControlledAgent> [stepUp()] Current: (", String.valueOf(p.getX())+","+String.valueOf(p.getZ())+");" +
								" Next: ("+ String.valueOf(x)+","+String.valueOf(z)+")"+
								"this.areIntoRange("+x+","+z+") = "+this.areIntoRange(x, z));
		if(this.areIntoRange(x, z)){
			Point3d currentPosition = new Point3d(),
					targetPosition =  new Point3d(x,0,z);
			this.getCoords(currentPosition);
			double distance = 0, lastDistance = Double.MAX_VALUE;
			boolean isSlopeDown = false;
			do{
				distance = (double)(targetPosition.distance(currentPosition));
				this.setTranslationalVelocity(1);
				SourceGetter.out.debug("ControlledAgent> [stepUp()] Step Forth: ",String.valueOf(distance));
				try {
					Thread.sleep((long)distance * 1000);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
				this.getCoords(currentPosition);
				isSlopeDown = (lastDistance>distance);
				lastDistance = distance;
			//}while(((int)distance*10) > 0);
			}while(isSlopeDown);
			
			this.setTranslationalVelocity(0);
			
//			timer.schedule(new TimerTask() {
//					public void run() {
//						ControlledAgent.this.timer.cancel();
//						ControlledAgent.this.setTranslationalVelocity(0);
//					}
//				}, this.stepForth * 1000, 1);
			
			/*this.moveToPosition(x,z);
			/*this.moveToPosition(0,0);
			this.rotateY(this.azimuth);
			this.moveToPosition(x,z);*/
		}
	}
	public void stepDown(){
//		if(this.stepStatus==this.GOING_FORTH&&this.step==this.STEP_INCREASE){
//			this.step=0;
//			this.stepStatus = !this.GOING_FORTH;
//		}else{
//			this.step-=(this.step<-3)?0:this.STEP_INCREASE;
//		}
	}
	public void turnLeft(){
//		if(this.turnningStatus==this.TURNNING_RIGHT&&this.turn==(-1*this.TURN_INCREASE)){
//			this.turn=0;
//			this.turnningStatus = !this.TURNNING_RIGHT;
//		}else{
//			this.turn+=(this.step>180)?0:this.TURN_INCREASE;
//		}
		this.rotateY(Math.PI/2);
		
		//Updating azimuth:
		double tmp = (this.azimuth + 90);
		this.azimuth = (tmp>=360)?0:tmp;
	}
	public void turnRight(){
//		if(this.turnningStatus!=this.TURNNING_RIGHT&&this.turn==this.TURN_INCREASE){
//			this.turn=0;
//			this.turnningStatus = this.TURNNING_RIGHT;
//		}else{
//			this.turn-=(this.step<-180)?0:this.TURN_INCREASE;
//		}
		this.rotateY(-1*Math.PI/2);
		
		//Updating azimuth:
		double tmp = (this.azimuth - 90);
		this.azimuth = (tmp<0)?270:tmp;
	}
	
	/**
	 * @return azimuth.
	 */
	public double getAzimuth(){
		return this.azimuth;
	}
	/**
	 * This method allows to know current agent azimuth.
	 * @param currentX: Current x.
	 * @param currentZ: Current z.
	 * @return azimuth.
	 */
//	public double getAzimuth(double currentX, double currentZ){
//		double azimut;
//		SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "last values = ("+this.lastX+","+this.lastZ+")");
//		if(this.stepStatus == this.GOING_FORTH){
//			azimut = this.getAzimuth(this.lastX, this.lastZ, currentX, currentZ);
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "GOING_FORTH = "+String.valueOf(azimut));
//		}else{
//			azimut = this.getAzimuth(currentX, currentZ, this.lastX, this.lastZ);
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "GOING_BACK = "+String.valueOf(azimut));
//		}
//		if(currentX != this.lastX && currentZ != this.lastZ){
//			this.lastX = currentX;
//			this.lastZ = currentZ;
//			SourceGetter.out.debug("ControlledAgent> [getAzimuth("+currentX+", "+currentZ+")]", "Updating last values.");
//		}
//		return azimut;
//	}
	
	/**
	 * This method allows to know current agent azimuth.
	 * @param x1: Last x.
	 * @param z1: Last z.
	 * @param x2: Current x.
	 * @param z2: Current z.
	 * @return azimuth.
	 */
//	public double getAzimuth(double x1, double z1, double x2, double z2){
//		double F = ((180/Math.PI)*Math.acos((x2-x1)/(Math.sqrt(Math.pow(x1-x2, 2)+Math.pow(z1-z2, 2)))));
//		return 360-(((z2-z1)<0)?(360-F):F);
//	}
}
